#include <avr/io.h>
#include <avr/pgmspace.h>
#include <stdint.h>
#include <util/delay.	>
#include <avr/interrupt.h>
#include <math.h>
#include "usb_serial.h"
#include "uartStrings.h"
#include "init.h"

#define debug 0
#define noRcInputs 2

#define newLine 0x0A
#define CPU_PRESCALE(n) (CLKPR = 0x80, CLKPR = (n))
#define CPU_16MHz       0x00
#define CPU_8MHz        0x01
#define CPU_4MHz        0x02
#define CPU_2MHz        0x03
#define CPU_1MHz        0x04
#define CPU_500kHz      0x05
#define CPU_250kHz      0x06
#define CPU_125kHz      0x07
#define CPU_62kHz       0x08

//#Definitions
#define rcMax (2.5*16000)
#define rcMin (0.5*16000)
#define serialThreshold 50  //How many messages can be missed before failsafing
#define rcThreshold 50  //Must get at least 5Hz (Timer runs ~ 250hz 250/50gives 5Hz)
#define centerMin  (1.4*16000)  //The region which is deemed the center to allow serial
#define centerMax  (1.6*16000)
#define waitTimeout (1*250) //no. seconds to have cenetered sticks * 250
#define gearRatio 12.89
#define velocityPeriod 0.065535

//States
#define failsafe 0
#define rc 1
#define serial 2

//Globals
volatile uint8_t rcHeartbeat = 0; //RC Heartbeat counter
volatile uint8_t rcTimeout = 0; //RC Timeout counter

volatile uint8_t serialTimeout = 0; //Serial Heatbeat counter
volatile uint8_t serialHeartbeat = 0; //Serial Timeout counter

volatile uint16_t time = 0; // Counts up @ ~250hz.

volatile uint8_t state = 0; //Output State
volatile uint16_t serOutputs[3] = {0};

volatile uint16_t motorCounts = 0;
volatile uint16_t prevMotorCounts = 0;
volatile uint16_t sendMotorCounts = 0;
volatile uint16_t countDiff = 0;
volatile uint8_t sendVoltage = 0;
volatile uint8_t sendCurrent = 0;




uint8_t flags = 0;
#define sentFlag 0
#define rxSerialFlag 1
#define rxLimitsFlag 2
#define newAdcVoltage 3
#define newAdcCurrent 4

//RC input definitions
uint8_t prevPinState = 0; //Holds state for pin change interrupt
uint16_t rcInputs[noRcInputs] = {0}; //Rc Input variable
uint16_t startTime[noRcInputs] = {0}; //Time counting started

int main(void) {
    // Variables
    uint8_t receiveCounter = 10;
    uint16_t waitTime = 0;
    // Initialisation 
    CPU_PRESCALE(CPU_16MHz); //Setup Speed

    //We will use the Pin Change interrupts as inputs could also use interrupts
    //but this is easier to allow varying of number of inputs d
    //These are all on PORTB
    DDRB = 0; //Set PortB to be inputs.
    DDRC = 0b01110000; //Enable timer3 outputs
    PORTB = 0xFF; //Also enable the internal pullups
    
    usb_init(); //Setup USB-UART

    //Setup Timer (will run @ approx 250Hz)
    TIMSK3 = 0x01; //Enable overflow interrupt
    ICR3 = 0xFFFF;
    TCCR3A = 0b10101010; //Fast PWM with Pin toggles enabled
    TCCR3B = 0b00011001; //No Prescaler

    OCR3A = 0x0000; //Initialize to Zero
    OCR3B = 0x0000;
    OCR3C = 0x0000;

    PCICR = 0x01; //Enable Pin Change Interrupts
    PCMSK0 = 0b00000011; //Enable interrupts on lower pins

    //Setup External Interrupts
    EIMSK = 0b00000111; //Enable interrupts
    EICRA = 0b00010101; //Enable INT0,1,2 on any edge.
    DDRD = 0b00000000; //Set inputs
    //PORTD = 0b11111111; //Enable internal pullups

    //Setup ADC for current sensor
    ADCSRA |= (1 << ADPS2) | (0 << ADPS1) | (1 << ADPS0); // Set ADC prescaler to 128 - 125KHz sample rate @ 16MHz
        
    ADMUX |= (1 << REFS0) | (1 << REFS1); // Set ADC reference to 2.56
    ADMUX |= (1 << ADLAR); // Left adjust ADC result to allow easy 8 bit reading
        
    ADMUX |= 0; //Set ADC to temp sensor
        
    ADCSRA |= (1 << ADATE); // Set ADC to Free-Running Mode
    ADCSRA |= (1 << ADEN); // Enable ADC
    ADCSRA |= (1 << ADIE); // Enable ADC Interrupt
    ADCSRA |= (1 << ADSC); // Start A2D Conversions

    sei();
    
    //Main Loop
    while (1) {
        //Read in serial data if available
        while (usb_serial_available()) {
            uint8_t temp = usb_serial_getchar();
            if (temp == 0xFE) {
                receiveCounter = 0;
                if (debug){
                    usb_serial_write("Serial Start\n = ",13);
                }
            } else if (temp < 240 && temp > 60 && receiveCounter <= noRcInputs){
                //Load data into serial outputs
                //Expects character from 60 - 240
                serOutputs[receiveCounter] = temp*160;

                if (receiveCounter == (noRcInputs-1)) {
                    //We have successfully received one set of commands
                    serialHeartbeat++;
                    if (debug){
                        usb_serial_write("Got Serial\n = ",11);
                        printUint(temp);
                        newline();
                    }
                }
                receiveCounter++;
            }
        }
                
        //Need to manage which mode we are in.
        //This assumes that if we are receiving serial data and sticks have been
        //centered for 1s then it is ok to enter serial mode.
        if (rcTimeout < rcThreshold && serialTimeout < serialThreshold) {
            //if we have valid data
            if (rcInputs[0] > centerMin && rcInputs[0] < centerMax
                    && rcInputs[1] > centerMin && rcInputs[1] < centerMax) {
                if (time - waitTime > waitTimeout) {
                    state = serial;
                }
            } else {
                waitTime = time; //set new start time
                state = rc;
            }
        } else if (rcTimeout < rcThreshold ) {
            state = rc; //if we have valid RC data then use it.
            waitTime = time; //set new start time
        } else {
            waitTime = time; //set new start time
        }

        //send velocity
        if (!(time % 4)){
            if (!(flags & (1 << sentFlag))){ //if flag is not set
                //send velocity
                usb_serial_putchar_nowait('V');
                usb_serial_putchar_nowait(',');
                printUint(sendMotorCounts);
                usb_serial_putchar_nowait(',');
                //send RC values
                printUint(OCR3A);
                usb_serial_putchar_nowait(',');
                printUint(OCR3B);
                //usb_serial_putchar_nowait(',');
                //printUint(rcInputs[0]);
                ///usb_serial_putchar_nowait(',');
                //printUint(rcInputs[1]);
                usb_serial_putchar_nowait(',');
                printShort(sendVoltage);
                usb_serial_putchar_nowait(',');
                printShort(sendCurrent);
                newline();
                
                flags |= (1 << sentFlag); //set Flag
            }
        }
        else {
             flags &= ~(1 << sentFlag); //clear Flag
        }        
    }
}

//Pin change interrupt
ISR(PCINT0_vect) {
    //Read in Pins
    char Pins = PINB;
    char temp = Pins ^ prevPinState; //XOR pins

    //Process pins have changed.
    uint8_t i = 0;
    while (temp != 0) //Check pins until there is no changes left
    {
        if (temp & (1 << i)) {
            //Bit i has changed
            if (Pins & (1 << i)) {
                //Bit i has gone high
                //Read in timer value and save
                startTime[i] = TCNT3;
            } else {
                //Bit i has gone low
                uint16_t currentTime = TCNT3;
                currentTime -= startTime[i];
                if (currentTime < rcMax && currentTime > rcMin) {
                    //We have a valid pulse
                    rcInputs[i] = currentTime;
                    rcHeartbeat++; //Shift right until 0;
                }
            }
        }
        temp &= ~(1 << i);
        i++;
    }
    prevPinState = Pins; //Save pins for next time
}

ISR(TIMER3_OVF_vect) { 
    time++; //Increment time counter @ 250Hz
    if (!(time % 4)){
        sendMotorCounts = motorCounts;
    }
    
    if (serialHeartbeat == 0) {
        if (serialTimeout < serialThreshold){
            serialTimeout++;
        }
    } else {
        serialTimeout = 0;
        serialHeartbeat = 0; //Clear beat
    }

    if (rcHeartbeat == 0) {
        if (rcTimeout < rcThreshold) {
            rcTimeout++;
        }
    } else {
        rcTimeout = 0;
        rcHeartbeat = 0; //Clear beat
    }

    //Check states still valid
    if (state == serial) {
        //Need valid RC and serial
        if (serialTimeout >= serialThreshold) {
            state = rc; //If we dont have them failsafe
       }
    } else if (state == rc) {
        //Need valid RC signal
        if (rcTimeout >= rcThreshold) {
            state = failsafe;
        }
    }

    //Load values
    if (state == failsafe) {
        //Failsafe
        OCR3A = 0;
        OCR3B = 0;
        OCR3C = 0;
    } else if (state == rc) {
        //RC Mode
        //TODO this will fail if <3 RC inputs
        OCR3A = rcInputs[0];
        OCR3B = rcInputs[1];
        OCR3C = rcInputs[2];
    } else if (state == serial) {
        //Serial Mode
        OCR3A = serOutputs[0];
        OCR3B = serOutputs[1];
        OCR3C = serOutputs[2];
    } else {
        //If we somehow ever get to an unknown state set failsafe
        state = failsafe;
        OCR3A = 0;
        OCR3B = 0;
        OCR3C = 0;
    }

}

ISR(INT0_vect){
    if (PIND & (1 << 0)) //If PORTD0 is high
    {
        if(PIND & (1 << 2)){
            //Counting up
            motorCounts++;
            //usb_serial_putchar_nowait('+');
        } else if (PIND & (1 <<1)){
            //Counting Down
            motorCounts--;
            //usb_serial_putchar_nowait('-');
        }
    }
    else { //If PORTD0 is low
        if (PIND & (1 << 2))
        {
            //Counting down
            motorCounts--;
            //usb_serial_putchar_nowait('-');
        } else if (PIND & (1 <<1)){
            //Counting up
            motorCounts++;
            //usb_serial_putchar_nowait('+');
        }
    }
}

ISR(INT1_vect){
    if (PIND & (1 << 1)) //If PORTD0 is high
    {
        if (PIND & (1 << 0))
        {
            //Counting up
            motorCounts++;
            //usb_serial_putchar_nowait('+');
        } else if (PIND & (1 <<2)){
            //Counting Down
            motorCounts--;
            //usb_serial_putchar_nowait('-');
        }
    }
    else { //If PORTD0 is low
        if (PIND & (1 << 0))
        {
            //Counting down
            motorCounts--;
            //usb_serial_putchar_nowait('-');
        } else if (PIND & (1 << 2)){
            //Counting up
            motorCounts++;
            //usb_serial_putchar_nowait('+');
        }
    }
    if(debug){
        for (char i = 0; i<3; i++){
            if ((PIND & (1 << i)) > 0){
                usb_serial_putchar_nowait('1');
            }
            else {
                usb_serial_putchar_nowait('0');
            }
        }
        newline();
    }
}

ISR(INT2_vect){
    if (PIND & (1 << 2)) //If PORTD0 is high
    {
        if (PIND & (1 << 1))
        {
            //Counting up
            motorCounts++;
            //usb_serial_putchar_nowait('+');
        } else if (PIND & (1 << 2)){
            //Counting Down
            motorCounts--;
            //usb_serial_putchar_nowait('-');
        }
    }
    else { //If PORTD0 is low
        if (PIND & (1 << 1))
        {
            //Counting down
            motorCounts--;
            //usb_serial_putchar_nowait('-');
        } else if (PIND & (1 << 2)){
            //Counting up
            motorCounts++;
            //usb_serial_putchar_nowait('+');
        }
    }
    if(debug){
        for (char i = 0; i<3; i++){
            if ((PIND & (1 << i)) > 0){
                usb_serial_putchar_nowait('1');
            }
            else {
                usb_serial_putchar_nowait('0');
            }
        }
        newline();
    }
}


volatile uint8_t adcCounter = 0;
ISR(ADC_vect){
    if (adcCounter == 5){
        sendVoltage = ADCH;
        flags |= (1 << newAdcVoltage); 
        ADMUX |= (1 << 0); //Set to ADC1
    } else if (adcCounter == 15){
        sendCurrent = ADCH;
        flags |= (1 << newAdcCurrent); 
        ADMUX &= ~(1 << 0); //Set to ADC0
        adcCounter = 0;
    } else if (adcCounter == 25){
        adcCounter = 0;
    }    
    adcCounter++;
    
}
